/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/trajectory_navigation.h
 * @Description  : TrajNv类，该类负责实现路径导航的功能，包括轨迹生成、速度控制、导航状态管理等。    
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:53:09
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once

#include "trajectory.h"
#include "profiler.h"
#include "guide_help.h"
#include "node.h"
#include "world.h"

enum TagWorkMode
{
    NV_NORMAL,
    NV_SUSPEND
};

// Define navigation level macro
enum TagNavLevel
{
    NAV_LEVEL_NORMAL = 1,
    NAV_LEVEL_DEAD_RECKON = 2,
    NAV_LEVEL_FAIL = 3
};

constexpr int SMART_TURN_DIR = 2;                  // Smart turn direction
constexpr int BRAKE_DELAY_COUNT = 30;              // Brake delay count: 30 (50ms * 30 = 1500ms = 1.5s)
constexpr float STOP_VEL_THRESHOLD = 0.001f;       // Stop velocity threshold
constexpr float MAX_STEER_ANGLE_SMOOTH = 15.0f;    // Maximum smooth steering angle 
constexpr float STEERING_VEL_THRESHOLD = 0.0001f;  // Steering velocity threshold
constexpr float MIN_PATH_FOLLOW_VEL = 0.01f;       // Minimum path follow velocity   
constexpr float MAX_DEAD_SPIN_ANGLE_DEGREE = 90.0f;// Maximum allowed rotation angle after localization loss
constexpr float ADD_ON_SURE_AREA = 0.01f;          // Add-on sure area
constexpr float ADD_ON_SURE_AREA_FOR_SPP = 0.05f;  // Add-on sure area for SPP 
constexpr int IGNORE_Y_ERROR_BITS = 14;            // Force ignore Y direction error            
constexpr int CURVATURE_CHECK_MARK = 1;            // curvature check mark
//////////////////////////////////////////////////////////////////////////////
//   The interface of class "TrajectoryNv".
class TrajNv
{
 public:
    enum BrakeStage  {
        BRAKE_UNLOCK = 0,               // 打开抱闸阶段
        START_BRAKE_UNLOCK = 1,         // 开始打开抱闸阶段
        BRAKE_LOCK = 2                  // 抱闸关闭(锁住)
    };
    
    // TrajNv constructor
    TrajNv(Path *path, Node &to_node, float from, float to, short level,
        bool force_check_mark = false, bool use_speed_level = true,
        bool use_temp_end_point = false);

    // Overloaded constructor, only for spin turn
    TrajNv(Node *node, Angle &ang_start, Angle &ang_end, short level, 
        bool sync_type, int turn_dir = SMART_TURN_DIR, 
        bool use_speed_level = true, bool check_mark = false, 
        float area = 0.0f);
    
    // TrajNv destructor
    virtual ~TrajNv() {
        if (traj_pointer_ != nullptr) {
            delete traj_pointer_;
            traj_pointer_ = nullptr;
        }
    }

    // Create the trajectory navigation object
    void Create(float max_value, float refrain_value, float up_slope, float down_slope,
                float up_small_slope, float down_small_slope, float transition_value, float sure_area, float search_0_value,
                short guide_type, int check_mark, float max_dead_distance = 0);
    
    // Initialize the navigation
    void Start(float start_value, float end_value, short work_mode,
               float init_curvature, bool init_curvature_known, short last_traj_type, Angle last_steer_angle);
    
    // Get trajectory type
    short GetTrajType();
    
    // Called when the navigation is started
    virtual void OnStart();
    
    // Called before calling the function "CollectInfo"
    virtual void PreCollectInfo();
    
    // Generate drive velocity
    virtual void GenerateVelocity();
    
    // Called before generating the vehicle's velocity vector
    virtual void PreGenerateVelocity();
    
    // Called after generating the vehicle's velocity vector
    virtual void AfterGenerateVelocity();
    
    // The routine to be called in each working cycle
    virtual void CycleRoutine();
    
    // Collect necessary information
    virtual short CollectInfo() { return 0; }
    
    // Drive the vehicle
    virtual void Drive();
    
    // Test whether the navigation has finished or not
    virtual bool Finished() { return finished_; }
    
    // Called before the navigation is started
    virtual void PreStart(float start_value, float end_value, short work_mode);
    
    // Called when the navigation is finished
    virtual void OnFinish(); 
    
    // Get guide type
    short GetGuideType() { return guide_type_; } // The type of navigation
    
    // Modify the end value of the profile
    void ModifyEndValue(float new_end_value) { profiler_.SetEndValue(new_end_value); }
    
    // Get the end value of the profile
    float GetEndValue() { return profiler_.GetEndValue(); }
    
    // Get flag to force override end velocity for dynamic operation
    bool OverrideEndVel() { return override_end_velocity_; }
    
    // Set flag to force override end velocity for dynamic operation
    void SetOverrideEndVel(bool override_flag) { override_end_velocity_ = override_flag; }
    
    // Get the pointer to the trajectory
    Trajectory* GetTraj() { return traj_pointer_; }
    
    // Get the current steer angle
    Angle GetSteerAngle() { return traj_pointer_->SteeringFun(); }
    
    // Get the current posture
    Posture& GetPosture() { return traj_pointer_->PostureFun(); }
    
    // Get the current linear velocity
    float GetLinearVel() { return velocity_.linear_; }
    
    // Get the current angular velocity
    float GetAngularVel() { return velocity_.angular_; }
    
    // Get the current planned speed
    float GetCurValue() { return rate_; }
    
    // Get path pointer to the path
    Path* GetPath() { return path_pointer_; }
    
    // Get the current linear cruise velocity
    float LinearCruiseVel();
    
    // Get the current linear refrain velocity
    float LinearRefrainVel();
    
    // Get the current remaining distance
    float GetCurRemainArea() { return area_; }
    
    // Get the current running distance
    float GetRelProgress() { return real_progress_; }
    
    // Get the start curvature of the navigation
    float StartCurvature() { return traj_pointer_->StartCurvature(); }
    
    // Get the end curvature of the navigation
    float EndCurvature() { return traj_pointer_->EndCurvature(); }
    
    // Get the current curvature of the navigation
    float Curvature() { return traj_pointer_->CurvatureFun(); }
    
    // Get the current move direction
    MoveDir GetMoveDir() { return traj_pointer_->WheelMoveDir(); }
    
    // Get the current work mode
    short GetWorkMode() { return work_mode_; }
    
    // Check if steering angle is at the target position
    bool SteerReadyOk();
    
    // Check if rack follow-up is enabled
    bool CanRackFollowUp();
    
    // Set planner
    bool LoadPlanner(OfflineScurvePlan *planner) { return profiler_.SetUpPlanner(planner); }
    
    // Get maximum speed limit for the path
    float GetMaxVel() { return max_value_; }

 public:
    Trajectory *traj_pointer_;         // Pointer to the trajectory
    Node *spin_node_pointer_;          // The node where to do spin turn
    Profiler profiler_;                // The profiler object
    Velocity velocity_;                // The current control velocity
    Angle steer_angle_;                // The current steer angle for drive complex or shift mode

    short old_work_mode_;               // The old work mode
    float rate_;                        // The rate of progress
    float real_progress_;               // The progress
    float area_;                        // The area left
    float desired_spin_progress_;       // The desired spin progress
    float max_value_;                   // Maximum value of profile
    float refrain_value_;               // Refrain value of the profile
    float up_slope_;                    // Going-up slope
    float down_slope_;                  // Going-down slope
    float up_small_slope_;              // Up small slope
    float down_small_slope_;            // Down small slope
    float transition_value_;            // Transition value
    float sure_area_;                   // Making-sure area
    float search_0_value_;              // Search zero value
    float cycle_;                       // Control cycle
    bool is_steer_ok_;                  // Whether the kinematic form is OK (target steer angle is ok) 
    bool finished_;                     // Whether the navigation has finished or not

    BrakeStage brake_stage_;            // Current brake stage
    short base_guide_type_;             // guide type
    short guide_type_;                  // The type of navigation
    short work_mode_;                   // The current work mode
    bool is_steering_;                  // Whether steering command is being sent during stop recovery
    bool need_add_area_;                // need add area
    bool last_nv_need_stop_;            // last navigation need stop
    bool override_end_velocity_;        // For dynamic operation: override end value (0.0)
    float max_dead_distance_;           // Maximum dead distance: Navigation parameter
    int brake_count_;                   // Brake count
    Node to_node_;                      // Temporary variable (useless)

    unsigned short start_node_id_;      // ID of the start node
    unsigned short end_node_id_;        // ID of the end node

    static bool init_posture_known_;    // Flag - Initial posture is known
    static Posture static_posture_;     // Current posture for gyro or laser helper to update real-time posture

 protected:
    bool ignore_y_error_;               // Ignore Y direction error bit 14
    GuideHelper guide_helper_;         // The guide helper

 private:
    // Turn model drive
    void DriveTurn();

    // Shift model drive
    void DriveShift() {};

    // Check if the vehicle is stopped
    bool IsStop(SHORT traj_type);

    // Suppress planning
    bool ProfilerRefrain(short traj_type);

    // Output velocity for planning
    void ProfilerOutVel(float &current_value) {
        float limit_max_rate = traj_pointer_->GetLimitRate(max_value_);
        current_value = profiler_.LimitGenerate(limit_max_rate); // Important
    }

    // Generate progress velocity and steer angle
    void GenerateProgVelSteerAngle();

    // Check if new trajectory curvature matches the last one smoothly
    void CheckCurvatureSmooth(Trajectory *traj, short last_traj_type, bool init_curvature_known, float init_curvature, Angle last_steer_angle);
    
    // Verify if trajectory object is valid
    bool VerifyTrajObj() {
        if (!traj_pointer_) {
            velocity_.SetZero();
            DEBUG_ERROR_OUT("Error: VerifyTrajObj traj_pointer_ is null");
            return false;
        }
        return true;
    }

    // Check if it is a spin node and node object
    Node* GetSpinNodeObj();

    // Steering logic for starting from a standstill: true if completed, false if ready
    bool Steering(Trajectory *traj);

    // Get the deviation of navigation
    void GetNavDeviation(float& dev_x, float& dev_y, float& dev_t);
    
    // Get the current navigation level
    TagNavLevel GetNavLevel();

    // Update the vehicle status
    void UpdateVehicleStatus();

    // No guide reload area
    void NoGuideReloadArea();

    // Guide Reload area
    void GuideReloadArea();

    // Generate angular velocity
    void LaserGenAngularVel();

    // Generate angular velocity for guide
    void GuideGenAngularVel();

    // Create the helper object
    void HelperCreate(int check_mark);

    // Helper object initialization
    void HelperOnStart();

    // Check if spin angle exceeds tolerance
    bool CheckSpinPathLost(bool reset);

    // Validate path landmarks and odometry distance
    void PathVerify();

    // Limit vehicle speed in all directions to prevent excessive steering angle
    void LimitModifyVelocity(const Velocity &planned_velocity, ModifyCtrl &modify);

    // Start laser detect area and nav mode 
    bool StartLaserDetectAreaAndNavMode(Path *path, Trajectory *traj);

    Path *path_pointer_;  // Pointer to the path about world
    bool use_speed_level_;  // Whether to use speed level or not
    bool online_;  // Flag - work in ONLINE mode
    short level_;  // Velocity level
    bool force_check_mark_;  // Force check mark
    bool use_temp_end_point_; 
};
